﻿using System;
using System.IO.Ports;

namespace blue.connection.mag_guide
{
    class MagGuideLog {
        public int urt_cnt=0; // Count for call updateSensorDataRT.
        public int urt_fail_lack=0; // count for "lack of data".
        public int frame_cnt = 0; // count for "read frame"
        public int fail_cs=0; // count for "checksum error".
        public int fail_tail=0; //count for "tail is error".
        public int fail_header=0; // count for "header is error".
    }

    /// <summary>
    /// connection layer for magnetic guide sensor.
    /// </summary>
    class MagGuideCon
    {
        private string portname_;
        private int baudrate_;
        private SerialPort serialport_;
        public MagGuideCon(string portname, int baudrate = 115200)
        {
            portname_ = portname;
            baudrate_ = baudrate;

            serialport_ = new SerialPort();
            // Allow the user to set the appropriate properties.
            serialport_.PortName = portname_;
            serialport_.BaudRate = baudrate_;
            serialport_.ReadTimeout = 0;

            serialport_.Open();
        }
        public void copy(byte[] source, int sstart, int slen, byte[] dest, int dstart)
        {
            for (int i = 0; i < slen; i++)
            {
                dest[dstart + i] = source[sstart + i];
            }
        }
        /// <summary>
        /// 尝试读取数据，将剩余的data填满。如果数据不足，就尽可能的填，返回false。
        /// 读取指定数量的字节。出于健壮性考虑，考虑了下面情况：
        ///     1.单次读取不完全；
        ///     2.当前数据不足，达到timeout后，直接返回null（将改为非阻塞方式）；
        ///     3.如果数据不足以构成一个完整数据帧，则追加到data[pos]；
        /// </summary>
        /// <param name="data"></param>
        /// <param name="start_pos">数据写入的起始位置</param>
        /// <returns>返回data中的有效字节数</returns>
        public int read(byte[] data, int start_pos)
        {
            if (serialport_ == null)
            {
                throw new ArgumentNullException("Use a invalid Serialport");
                // return false;
            }

            int cur_pos = start_pos;
            int max_pos = data.Length; // Max position for write.

            try
            {
                while (cur_pos < max_pos )
                {
                    //show("start recv_data ==============="); // Read time
                    int recv_cnt = serialport_.Read(data, cur_pos, max_pos-cur_pos);
                    //show("recv_data ===============" + BitConverter.ToString(recv_data,0)); // Read Data
                    //show("data ===============<"+ cnt + ">"+ BitConverter.ToString(data, 0)); // Concate Data
                    cur_pos += recv_cnt;
                }
                return cur_pos;
            }
            catch (TimeoutException)
            {
                return cur_pos;
            }
        }

        public bool write(byte[] data, int pos = 0)
        {
            if (serialport_ == null)
            {
                //throw new ConnectionException("Use uninitialized magnetic guide connection.");
                return false;
            }
            serialport_.Write(data, pos, data.Length - pos);
            return true;
        }

        public void show(string msg)
        {
            string time = System.DateTime.Now.ToString("hh:mm:ss.fff");
            time += " > ";
            time += msg;
            System.Console.WriteLine(time);
        }
    }

    /// <summary>
    /// protocol layer for magnetic guide sensor.
    /// </summary>
    class MagProtocol
    {
        MagGuideLog log_;
        MagGuideCon con_;
        private byte[] body = new byte[16];  // header+cmd+data+cs+tail
        private byte[] recv_buff = new byte[16];
        private int recv_cnt = 0;   // Valid data at receive buffer.
        private int body_len = 16;
        private readonly byte header = 0x55;
        private readonly byte io_status = 0x00;
        private readonly byte set_period = 0x03;     // Set period for sensor.
        private readonly byte set_period_rep = 0x04; // Response for set period.
        private readonly byte get_ver = 0x05;        // Get Version information.
        private readonly byte get_ver_rep = 0x06;    // Response for get version.
        private readonly byte tail = 0xFE;
        private string version_; // Protocol version
        private byte[] period_ = new byte[2];  // sensor work peroid.

        public MagProtocol(string portname, ushort period, string version = "V010")
        {
            version_ = version;
            log_ = new MagGuideLog();
            con_ = new MagGuideCon(portname);
            period_ = TransformUshortToByte(period, period_);
        }
        public bool verifyVersion()
        {
            //TODO
            return true;
        }
        public bool setPeriod(ushort period)
        {
            //TODO
            //Verify response message.
            period_ = TransformUshortToByte(period, period_);

            byte[] cmd = new byte[body_len];
            cmd[0] = header;
            cmd[1] = set_period;
            cmd[2] = period_[0];
            cmd[3] = period_[1];
            cmd[15] = tail;
            cmd[14] = computeCheckSum(cmd);
            // Send Cmd
            System.Console.WriteLine("Set Period: "+System.BitConverter.ToString(cmd));
            if (!con_.write(cmd))
            {
                System.Console.WriteLine("Set period failed [at Write data].");
                return false;
            }
            // Receive Cmd
            System.Threading.Thread.Sleep(100);
            if (con_.read(cmd, 0) != cmd.Length)
            {
                System.Console.WriteLine("Set Period failed [at read response]");
                return false;
            }

            return true;
        }
        public bool verifyMessage(byte[] data)
        {
            if (data[0] != header || data[body_len-1] != tail)
            {
                log_.fail_tail++;
                return false;
            }
            if (!verifyCheckSum(data))
            {
                log_.fail_cs++;
                return false;
            }

            return true;
        }

        public byte[] getRawData()
        {
            return body;
        }
        public bool updateSensorData()
        {
            body = null;
            if (readFrame(recv_buff,recv_cnt)) {
                body = recv_buff;
                recv_buff = new byte[body_len];
                recv_cnt = 0;
                return true;
            }
            return false;
        }
        /// <summary>
        /// Update sensor data to the last one. Theory is : keep read data frame untill we get the last one.
        /// In this case, we assume that all data is a integrated frames.
        /// </summary>
        /// <returns></returns>
        public bool updateSensorDataRT()
        {
            log_.urt_cnt++;
            // clear old data
            body = null;
            while (true)
            {
                if (!readFrame(recv_buff, recv_cnt)) {
                    break;
                }
                body = recv_buff;
                recv_buff = new byte[body_len];
                recv_cnt = 0;
            }

            if (body == null) log_.urt_fail_lack++;

            return body == null ? false : true;
        }

        public ushort getForward1Sensor()
        {
            return (ushort)((body[2] << 8) + body[3]);
        }
        public ushort getForward2Sensor()
        {
            return (ushort)((body[4] << 8) + body[5]);
        }

        public ushort getBehind1Sensor()
        {
            return (ushort)((body[6] << 8) + body[7]);
        }

        public ushort getBehind2Sensor()
        {
            return (ushort)((body[8] << 8) + body[9]);
        }

        public ushort getLeftSensor()
        {
            //return (ushort)((body[10] << 8) + body[11]); 下位机数据给反了
            return (ushort)((body[11] << 8) + body[10]);
        }

        public ushort getRightSensor()
        {
            //return (ushort)((body[12] << 8) + body[13]); 下位机数据给反了
            return (ushort)((body[13] << 8) + body[12]);
        }

        public string getLog() {
            return string.Format("Magnetic Guide Log > \r\n"
                            +"call_count(UpdateSensoDataRT) = {0}\r\n"
                            +"call UpdateSensorDataRT fail(lack data) = {1}\r\n"
                            +"frame count(all) = {2}\r\n"
                            +"frame fail(tail) = {3}\r\n"
                            +"frame fail(cs) = {4}\r\n"
                            +"frame fail(header) = {5}",log_.urt_cnt, log_.urt_fail_lack,
                            log_.frame_cnt,log_.fail_tail, log_.fail_cs, log_.fail_header);
        }

        public void show(string msg)
        {
            string time = System.DateTime.Now.ToString("hh:mm:ss.fff");
            time += " > ";
            time += msg;
            System.Console.WriteLine(time);
        }

        /// <summary>
        /// 将"xx xx 55 xx xx xx" 移动到 "55 xx xx xx 00 00"
        /// </summary>
        /// <param name="data"></param>
        /// <return>返回剩余数据数量。</return>
        private int removeToHeader(byte[] data, int start_index=0) {
            int header_pos = start_index; // Header position.
            int max_pos = data.Length;

            // Find Header position.
            while (header_pos < max_pos && data[header_pos] != header ) header_pos++;
            // Shift data
            int start_pos = 0;
            while (header_pos < max_pos && header_pos<max_pos) {
                data[start_pos++] = data[header_pos++];
            }
            // Clear the rest data
            int data_cnt = start_pos;
            while (start_pos<max_pos) { data[start_pos++] = 0; }

            return data_cnt;
        }

        private int removeToNextHeader(byte[] data) {
            return removeToHeader(data,1);
        }

        /// <summary>
        /// Read a valid frame.
        /// </summary>
        /// <param name="data"></param>
        /// <returns>true if success. false if false</returns>
        private bool readFrame(byte[] data, int data_cnt)
        {
            while (true)
            {
                data_cnt = con_.read(data, data_cnt);
                //show("serial data [" + data_cnt + "] " + BitConverter.ToString(data));
                if (data_cnt != data.Length) { return false; } // Haven't enough data.
                log_.frame_cnt++;

                data_cnt = removeToHeader(data);
                if (data_cnt < data.Length) {log_.fail_header++; continue; }

                if (!verifyMessage(data))
                {
                    removeToNextHeader(data);
                    continue;
                }

                break;
            }

            return true;
        }

        private bool verifyCheckSum(byte[] data)
        {
            byte checksum = data[14];
            return computeCheckSum(data) == checksum;
        }

        private byte computeCheckSum(byte[] data)
        {
            byte checksum = data[14];
            int sum = 0 - checksum;
            for (int i = 0; i < data.Length; i++)
            {
                sum += data[i];
            }
            return (byte)(sum & 0xff);
        }

        /// <summary>
        /// 将ushort按照协议要求转换成对应的byte序列。
        /// </summary>
        private byte[] TransformUshortToByte(ushort source, byte[] target)
        {
            target[0] = (byte)((source & 0xff00) >> 8);  // High Byte
            target[1] = (byte)(source & 0xff);         // Low Byte
            return target;
        }
    }

    enum SensorID
    {
        FORWARD1 = 0,
        FORWARD2,
        BEHIND1,
        BEHIND2,
        LEFT,
        RIGHT
    }
    /// <summary>
    /// Application layer.
    /// </summary>
    class MagneticGuide
    {
        MagProtocol protocol_;
        //Sensor maybe mounted at forward/reverse. We needs unify 
        //them to standard direction.If true, meansing need to reverse  bits.
        private bool[] direcs = new bool[6] { false, true, false, true,false,true};

        private int getUShortBit(ushort data, int bit_pos)
        {
            return (0x1 << bit_pos) & data;
        }
        private double computeCenter(ushort data)
        {
            int bit_size = sizeof(ushort) * 8;
            // Find first one
            int first = 0;
            for (first = 0; first < bit_size; first++)
            {
                if (getUShortBit(data, first) == 0x0)
                {
                    break;
                }
            }
            // Find last one
            int last = 15;
            for (; last >= 0; last--)
            {
                if (getUShortBit(data, last) == 0x0)
                {
                    break;
                }
            }

            return first <= last ? (first + last) / 2.0 : -1; // If Error, return -1.
        }

        private double computeCenterFromLeft(ushort data) {
            int bit_size = sizeof(ushort) * 8;
            // Find first zero
            int left = 0;
            for (left = 0; left < bit_size; left++)
            {
                if (getUShortBit(data, left) == 0x0)   break;
            }
            // Find last zero
            int right = left;
            for (; right < bit_size; right++) {
                if (getUShortBit(data, right) != 0x0) {
                    break;
                }
            }
            right -= 1;

            return left<bit_size ? (left + right) / 2.0 : -1; // If Error, return -1.
        }

        private double computeCenterFromRight(ushort data)
        {
            int bit_size = sizeof(ushort) * 8;
            // Find first zero
            int right = bit_size-1;
            for (; right>=0; right--)
            {
                if (getUShortBit(data, right) == 0x0) break;
            }
            // Find last zero
            int left = right;
            for (; left >=0 ; left--)
            {
                if (getUShortBit(data, left) != 0x0) {
                    break;
                }
            }
            left += 1;

            return right >= 0 ? (left + right) / 2.0 : -1; // If Error, return -1.
        }

        private double[] computeDoubleCenter(ushort data)
        {
            double[] pos = new double[2];
            pos[0] = computeCenterFromLeft(data);
            pos[1] = computeCenterFromRight(data);

            return pos;
        }

        private double reDirec(SensorID sensor_id,double pos) {
            if (pos < 0) return pos;
            int id = (int)sensor_id;
            return direcs[id] ? 15 - pos : pos;
        }

        private double[] reDirec(SensorID sensor_id, double[] pos)
        {
            int id = (int)sensor_id;
            if (direcs[id]) { // Reverse Directions
                double tmp = reDirec(sensor_id, pos[0]);
                pos[0] = reDirec(sensor_id, pos[1]);
                pos[1] = tmp;

            } else { //Forward Directions
                pos[0] = reDirec(sensor_id, pos[0]);
                pos[1] = reDirec(sensor_id, pos[1]);
            }

            return pos;
        }

        private static string ushortToString(ushort data)
        {
            return Convert.ToString(data, 2).PadLeft(16, '0');
        }

        public MagneticGuide(string portname, ushort period = 10)
        {
            protocol_ = new MagProtocol(portname, period, "V010");
            if (!protocol_.verifyVersion())
            {
                System.Console.WriteLine("Protocol Version Error");
                protocol_ = null;
                return;
            }
            if (!protocol_.setPeriod(20))
            {
                System.Console.WriteLine("Failed to Set Sensor Period.");
                protocol_ = null;
                return;
            }
        }

        public bool updateSensorData()
        {
            if (protocol_ == null) { return false; }
            return protocol_.updateSensorData();
        }

        public bool updateSensorDataRT()
        {
            if (protocol_ == null) { return false; }
            return protocol_.updateSensorDataRT();
        }

        public byte[] getRawData()
        {
            if (protocol_ == null)
            {
                return null;
            }
            return protocol_.getRawData();
        }

        public string[] getRawDataAsString() {
            string[] result = new String[6];
            // Sensor 1
            result[0] = ushortToString( protocol_.getForward1Sensor());
            // Sensor 2
            result[1] = ushortToString(protocol_.getForward1Sensor());
            // Sensor 3
            result[2] = ushortToString(protocol_.getForward1Sensor());
            // Sensor 4
            result[3] = ushortToString(protocol_.getForward1Sensor());
            // Sensor 5
            result[4] = ushortToString(protocol_.getForward1Sensor());
            // Sensor 6
            result[5] = ushortToString(protocol_.getForward1Sensor());

            return result;
        }

        public double getCenter(SensorID id)
        {
            if (protocol_ == null) return -1;
            double pos = -1;

            switch (id)
            {
                case SensorID.FORWARD1:
                    pos = computeCenter(protocol_.getForward1Sensor());
                    break;
                case SensorID.FORWARD2:
                    pos = computeCenter(protocol_.getForward2Sensor());
                    break;
                case SensorID.BEHIND1:
                    pos = computeCenter(protocol_.getBehind1Sensor());
                    break;
                case SensorID.BEHIND2:
                    pos = computeCenter(protocol_.getBehind2Sensor());
                    break;
                case SensorID.LEFT:
                    pos = computeCenter(protocol_.getLeftSensor());
                    break;
                case SensorID.RIGHT:
                    pos = computeCenter(protocol_.getRightSensor());
                    break;
                default:
                    return pos;
            }

            return reDirec(id, pos);
        }

        public double[] getDoubleCenter(SensorID id)
        {
            if (protocol_ == null) return new double[2] { -1,-1};
            double[] pos = new double[2] { -1,-1};

            switch (id)
            {
                case SensorID.FORWARD1:
                    pos = computeDoubleCenter(protocol_.getForward1Sensor());
                    break;
                case SensorID.FORWARD2:
                    pos = computeDoubleCenter(protocol_.getForward2Sensor());
                    break;
                case SensorID.BEHIND1:
                    pos = computeDoubleCenter(protocol_.getBehind1Sensor());
                    break;
                case SensorID.BEHIND2:
                    pos = computeDoubleCenter(protocol_.getBehind2Sensor());
                    break;
                case SensorID.LEFT:
                    pos = computeDoubleCenter(protocol_.getLeftSensor());
                    break;
                case SensorID.RIGHT:
                    pos = computeDoubleCenter(protocol_.getRightSensor());
                    break;
                default:
                    return pos;
            }
            pos = reDirec(id, pos);

            return pos;
        }

        public string getLog() {
            return protocol_.getLog();
        }
    }
}